#ifndef _DRIVE_H_
#define _DRIVE_H_

#include "rads_base.hpp"

#include <iostream>  
#include <sstream>  
#include <fcntl.h>  
#include <termios.h>  
#include <unistd.h>  
#include <cstring>  
#include <vector> 
#include <thread>
#include <cmath>
#include <chrono> 
#include <ctime> 
#include <condition_variable>

#include <ros/ros.h>
#include <ros/time.h>
#include <std_msgs/String.h>
#include <std_msgs/ByteMultiArray.h>
#include <geometry_msgs/Twist.h>
#include "serial/serial.h"
#include <rads/rads_scmd.h>
#include <rads/rads_scmdret.h>

/*
	关于下位机的上报通信数据协议说明 [ 下位机->上位机 ] , xor校验范围（biye0-byte23,从len开始,校验长度:24）
	协议总长度为27，数据长度25（除去长度值和XOR校验，有效数据长度23）
	ff fe 19 00 63 00 00 55 00 55 00 ff ff ff ff ff ff ff ff 00 00 00 00 00 00 01 7b
	ff fe [0] [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] xor
    帧头数据为2个byte，固定为ff fe
    [0]  = 0x19  [25个字节,最后一个是校验，实际数据长度为23]
	[1]  = model 		【0】
	[2]  = soc   		【0x63=99】
	[3]  = err			【0】
	[4]  = rpm-left-H8	【0】
	[5]  = rpm-left-L8	【0x55】
	[6]  = rpm-right-H8	【0】
	[7]  = rpm-right-L8	【0x55】
	[8]  = key(8bit)	【0】
	[9]  = ladar-1		【0xff】		// 单位:厘米(cm)
	[10] = ladar-2		【0xff】		// 单位:厘米(cm)
	[11] = ladar-3		【0xff】		// 单位:厘米(cm)
	[12] = ladar-4		【0xff】		// 单位:厘米(cm)
	[13] = ladar-5		【0xff】		// 单位:厘米(cm)
	[14] = ladar-6		【0xff】		// 单位:厘米(cm)
	[15] = ladar-7		【0xff】		// 单位:厘米(cm)
	[16] = ladar-8		【0xff】		// 单位:厘米(cm)
	[17] =
	[18] =
	[19] =
	[20] =
	[21] =
	[22] =
	[23] = heartbeat	【1】	// 每次自增1,0-255
	[24] = XOR			【0x7b】
*/

/*
    关于上位机下发的通信数据协议说明 [ 上位机 -> 下位机 ]
    协议总长度为27，数据长度25（除去长度值和XOR校验，有效数据长度23）
    ff fe 19 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 7b
    ff fe [0] [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] xor
    帧头数据为2个byte，固定为ff fe
    [0]  = len          【0x19】 [25个字节,最后一个是校验，实际数据长度为23]
    [1]  = model        【0】
    [2]  = steering-h 	【0】转向0-32767代表逆时针（反打方向盘左拐,转32769-65535代表顺时针（正大方向盘右拐
    [3]  = steering-l   【0】32767是0位，32768为原地掉头，32766左打32.766度，65535右打32.766度
    [4]  = acc			【0】油门值0-100
    [5]  = brake	    【0】制动值0-100
    [6]  = gear	        【0】0 = P , 1 = N , 2 = D , 3 = R , 4 = O 转速线控可以默认O（other）
    [7]  = target_speed	【0】单位:0.1km/h
    [8]  = rpml-h	    【0】0-32766正转，32767、32768转速0，32769-65535反转
    [9]  = rpml-l	    【0】这里的转速不是真实转速，而是模拟转速
    [10] = rpmr-h       【0】
    [11] = rpmr-l       【0】
    [12] = lignt        【0】【bit7急救灯【bit6警告灯【bit5作业灯【bit4行车灯【bit3右【bit2左【bit1后【bit0前转向灯
    [13] = audio        【0】【bit7 开/关所有【bit6-bit4 间隔时长播放(0-7秒)【bit3-bit0 最大支持16种播报
    [14] = sign         【0】 8组信号开关控制
    [15] = camera-hori-h【0】 水平转向范围,一般在0°至365°(有5°的覆盖角度，以消除监控死角)
    [16] = camera-hori-l【0】
    [17] = camera-ver   【0】 垂直转向范围,一般在+90°以内,0-126向下转动0-126°,129-255向上[-129]转动0-126°,127,128为0位
    [18] =
    [19] =
    [20] =
    [21] =
    [22] =
    [23] = heartbeat	【1】	// 每次自增1,0-255
    [24] = XOR			【0x7b】
 */

// 好好的一个串口由全双工变成半双工
struct SerialHalfDuplex{
    std::mutex sendMutex;
    std::atomic<bool> isSending;
};

// 超声波雷达，8个为一组
typedef struct
{
	uint16_t radar[RADAR_DATA_SIZE];
}UltrasonicRadar;

// 扩展位4byte
typedef struct
{
	uint8_t extdata[EXTEND_DATA_SIZE];
}ExtendData;

/*  差速控制读取
	0~32767 	-1 	顺时针旋转
	32768~65535 +1	逆时针旋转 */
typedef struct
{
	uint16_t left_fb_spd;	// feedback speed
	uint16_t right_fb_spd;	// feedback speed
	uint8_t battery_soc;
	uint8_t heartbeat;	
	UltrasonicRadar ultrasonic_radar;
	ExtendData extend;
}DiffControl;

/* 下位机的指令协议索引解析 */
enum FDRIVE_INDEX
{
	FDRIVE_LEN = 0,
    FDRIVE_MODEL = 1,
    FDRIVE_SOC = 2,
    FDRIVE_ERR = 3,
    FDRIVE_RPML_H = 4,
    FDRIVE_RPML_L = 5,
    FDRIVE_RPMR_H = 6,
    FDRIVE_RPMR_L = 7,
    FDRIVE_KEYS = 8,
    FDRIVE_LADAR_1 = 9,
    FDRIVE_LADAR_2 = 10,
    FDRIVE_LADAR_3 = 11,
    FDRIVE_LADAR_4 = 12,
    FDRIVE_LADAR_5 = 13,
    FDRIVE_LADAR_6 = 14,
    FDRIVE_LADAR_7 = 15,
    FDRIVE_LADAR_8 = 16,
    FDRIVE_UNKNOW_1 = 17,
    FDRIVE_UNKNOW_2 = 18,
    FDRIVE_UNKNOW_3 = 19,
    FDRIVE_UNKNOW_4 = 20,
    FDRIVE_UNKNOW_5 = 21,
    FDRIVE_UNKNOW_6 = 22,
    FDRIVE_HEARTBEAT = 23,
    FDRIVE_XORCHECK = 24,
};

typedef struct
{
 	unsigned char len;
    unsigned char model;
    unsigned char soc;
    unsigned char err;
    int rpm_l;
    int rpm_r;
    unsigned char key_1;
    unsigned char key_2;
    unsigned char key_3;
    unsigned char key_4;
    unsigned char key_5;
    unsigned char key_6;
    unsigned char key_7;
    unsigned char key_8;
    unsigned char ladar_1;
    unsigned char ladar_2;
    unsigned char ladar_3;
    unsigned char ladar_4;
    unsigned char ladar_5;
    unsigned char ladar_6;
    unsigned char ladar_7;
    unsigned char ladar_8;
    unsigned char unknow_1;
    unsigned char unknow_2;
    unsigned char unknow_3;
    unsigned char unknow_4;
    unsigned char unknow_5;
    unsigned char unknow_6;
    unsigned char heartbeat;
    unsigned char xor_check;
}FdriveData;


enum FCONTROL_INDEX
{
    FCONTROL_LEN = 2,
    FCONTROL_MODEL = 3,
    FCONTROL_STEERING_H = 4,
    FCONTROL_STEERING_L = 5,
    FCONTROL_ACC = 6,
    FCONTROL_BRAKE = 7,
    FCONTROL_GEAR = 8,
    FCONTROL_TARGET_SPEED = 9,
    FCONTROL_RPML_H = 10,
    FCONTROL_RPML_L = 11,
    FCONTROL_RPMR_H = 12,
    FCONTROL_RPMR_L = 13,
    FCONTROL_LIGHT = 14,
    FCONTROL_AUDIO = 15,
    FCONTROL_SIGN = 16,
    FCONTROL_CAMERAH_H = 17,
    FCONTROL_CAMERAH_L = 18,
    FCONTROL_CAMERAV = 19,
    FCONTROL_UNKNOW_1 = 20,
    FCONTROL_UNKNOW_2 = 21,
    FCONTROL_UNKNOW_3 = 22,
    FCONTROL_UNKNOW_4 = 23,
    FCONTROL_UNKNOW_5 = 24,
    FCONTROL_HEARTBEAT = 25,
    FCONTROL_XORCHECK = 26,
};

typedef struct
{
    unsigned char len;
    unsigned char model;
    int steering;
    unsigned char acc;
    unsigned char brake;
    unsigned char gear;
    unsigned char target_speed;
    int rpml;
    int rpmr;
    unsigned char light;
    unsigned char audio;
    unsigned char sign;
    int camera_hor;
    unsigned char camera_ver;
    unsigned char unknow_1;
    unsigned char unknow_2;
    unsigned char unknow_3;
    unsigned char unknow_4;
    unsigned char unknow_5;
    unsigned char heartbeat;
    unsigned char xor_check;
}FcontrolData;

// xor
int8_t xorChecksum(const uint8_t* data, size_t length) {
    uint8_t result = 0;
    for (size_t i = 0; i < length; ++i) {
        result ^= data[i];
    }
    return result;
}

class PlanningControl
{
    public:
        uint8_t data[DRIVE_BUFFER_SIZE+2];
    public:
        PlanningControl() : twist_(false), ackermann_(false) {
            std::fill(data, data + DRIVE_BUFFER_SIZE + 2, 0);
            data[0] = PROTOCOL_HEAD0;
            data[1] = PROTOCOL_HEAD1;
            data[2] = DRIVE_BUFFER_SIZE;
            data[7] = 100;
            data[8] = 4;
            data[9] = 0;
            uint8_t xorResult = xorChecksum(&data[2], DRIVE_BUF_XOR_SIZE);
            // 默认校验
            data[DRIVE_BUFFER_SIZE+1] = xorResult;
        }
        void twistCallback(const geometry_msgs::Twist& twist)
        {
            /*
            double vel_x = twist.linear.x;
            double vel_th = twist.angular.z;
            double left_vel = vel_x - vel_th * WHEEL_BASE_WIDTH / 2.0;
            double right_vel = vel_x + vel_th * WHEEL_BASE_WIDTH / 2.0;
            */
            twist_data = twist;
            twist_ = true;
            PlanningAllIn();
        }
        void ackermannCallback(const rads::rads_scmdret& scmdret)
        {
            scmdret_data = scmdret;
            ConvertSerialData();
            ackermann_ = true;
            PlanningAllIn();
        }
        void ConvertSerialData()
        {
            data[0] = PROTOCOL_HEAD0;
            data[1] = PROTOCOL_HEAD1;
            data[FCONTROL_LEN] = scmdret_data.len;
            data[FCONTROL_MODEL] = scmdret_data.model;
            data[FCONTROL_STEERING_H] = scmdret_data.steering / 256;
            data[FCONTROL_STEERING_L] = scmdret_data.steering % 256;
            data[FCONTROL_ACC] = scmdret_data.acc;
            data[FCONTROL_BRAKE] = scmdret_data.brake;
            data[FCONTROL_GEAR] = scmdret_data.gear;
            data[FCONTROL_TARGET_SPEED] = scmdret_data.target_speed;
            data[FCONTROL_RPML_H] = scmdret_data.rpml / 256;
            data[FCONTROL_RPML_L] = scmdret_data.rpml % 256;
            data[FCONTROL_RPMR_H] = scmdret_data.rpmr / 256;
            data[FCONTROL_RPMR_L] = scmdret_data.rpmr % 256;
            data[FCONTROL_LIGHT] = scmdret_data.lignt;
            data[FCONTROL_AUDIO] = scmdret_data.audio;
            data[FCONTROL_SIGN] = scmdret_data.sign;
            data[FCONTROL_CAMERAH_H] = scmdret_data.camera_hor / 256;
            data[FCONTROL_CAMERAH_L] = scmdret_data.camera_hor % 256;
            data[FCONTROL_CAMERAV] = scmdret_data.camera_ver;
            data[FCONTROL_UNKNOW_1] = scmdret_data.unknow_1;
            data[FCONTROL_UNKNOW_2] = scmdret_data.unknow_2;
            data[FCONTROL_UNKNOW_3] = scmdret_data.unknow_3;
            data[FCONTROL_UNKNOW_4] = scmdret_data.unknow_4;
            data[FCONTROL_UNKNOW_5] = scmdret_data.unknow_5;
            data[FCONTROL_HEARTBEAT] = scmdret_data.heartbeat;
            uint8_t xorResult = xorChecksum(&data[FCONTROL_LEN], DRIVE_BUF_XOR_SIZE);
            data[FCONTROL_XORCHECK] = xorResult;
        }
    private:
        void PlanningAllIn()
        {
            if (twist_ && ackermann_)
            {
                //
                twist_ = false;
                ackermann_ = false;
            }
        }
        geometry_msgs::Twist twist_data;
        rads::rads_scmdret scmdret_data;
        bool twist_;
        bool ackermann_;
};


class SerialPortDrive {
	public:  
		SerialPortDrive(const std::string& portName, speed_t baudRate): portName_(portName), baudRate_(baudRate), fd_(-1) {}  
		bool open() {  
			//fd_ = ::open(portName_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);  
			fd_ = ::open(portName_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
			if (fd_ == -1) {
				return false;  
			}  
			struct termios options;  
			tcgetattr(fd_, &options);  
			cfsetispeed(&options, baudRate_);  
			cfsetospeed(&options, baudRate_);  
			options.c_cflag &= ~PARENB; // No parity  
			options.c_cflag &= ~CSTOPB; // 1 stop bit  
			options.c_cflag &= ~CSIZE;  
			options.c_cflag |= CS8;     // 8 data bits  
			options.c_cflag |= (CLOCAL | CREAD);  
			options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  
			options.c_oflag &= ~OPOST;  
			tcsetattr(fd_, TCSANOW, &options);  
			struct termios tty;  
			tcgetattr(fd_, &tty);  
			tty.c_cc[VMIN]  = 0;  
			tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout  
			tcsetattr(fd_, TCSANOW, &tty);  
			fcntl(fd_, F_SETFL, 0);  
			return true;  
		}
        bool isOpen()
        {
            return fd_ != -1 ? true :false;
        }
		void close() {  
			if (fd_ != -1) {  
				::close(fd_);  
				fd_ = -1;  
			}  
		}
       
		bool writeData(const char* buffer,size_t length)
		{
			//if(-1 == fd_) return false;
			ssize_t bytesWrite = write(fd_,buffer,length);
			//if(-1 == bytesWrite) return false;
			//if(static_cast<size_t>(bytesWrite) != length)
		//	{
			//	DEBUG_ERROR("Partial write to serial port");	
			//	return false;
		//	}	
			return true;
		}
        // 读取下位机的数据
		int readData(uint8_t* data,size_t dataSize,bool xorCheck = false) {
		    const uint8_t frame_start[] = {PROTOCOL_HEAD0, PROTOCOL_HEAD1};
		    size_t frame_start_index = 0;
		    size_t bytesRead = 0;
		    size_t bufferIndex = 0;
		    while (bufferIndex < dataSize) {
		        uint8_t byte;
		        ssize_t num_read = read(fd_, &byte, 1);
		        if (num_read < 0) {
		            return -1;
		        } else if (num_read == 0) {
		            continue;
		        }
		        if (frame_start_index == 0 && byte == frame_start[0]) {
		            frame_start_index = 1;
		            bufferIndex = 0;
		        } else if (frame_start_index == 1 && byte == frame_start[1]) {
		            frame_start_index = 2;
		        }else if(frame_start_index == 2) {
		            data[bufferIndex++] = byte;
		            if (bufferIndex == dataSize) {
                        uint8_t xorResult = xorChecksum(data, DRIVE_BUF_XOR_SIZE);
                        if(xorCheck)
                        {
                            if(xorResult == data[bufferIndex-1])
                            {
                                frame_start_index = 0;
		                		bufferIndex = 0;
                            	break;
                            }else{
                                DEBUG_ERROR("Xor check failed, real value is %02x(err:%02x)",xorResult,data[bufferIndex-1]);
                                return -2;
                            }
                        }else{
		                	frame_start_index = 0;
		                	bufferIndex = 0;
                            break;
                        }
		            }
		        }
		    }
		    return 0;
		}
        // 数据流解析
		FdriveData toDriveData(uint8_t* data,size_t dataSize)
        {
            FdriveData fdd;
            fdd.len = data[FDRIVE_LEN];
            fdd.model = data[FDRIVE_MODEL];
            fdd.soc = data[FDRIVE_SOC];
            fdd.err = data[FDRIVE_ERR];
            fdd.rpm_l = (int)data[FDRIVE_RPML_H] * 256 + (int)data[FDRIVE_RPML_L];
            fdd.rpm_r = data[FDRIVE_RPMR_H] * 256 + (int)data[FDRIVE_RPMR_L];
            fdd.key_1 = (data[FDRIVE_KEYS] & 0x01) == 0? 0 : 1;
            fdd.key_2 = (data[FDRIVE_KEYS] & 0x02) == 0? 0 : 1;
            fdd.key_3 = (data[FDRIVE_KEYS] & 0x04) == 0? 0 : 1;
            fdd.key_4 = (data[FDRIVE_KEYS] & 0x08) == 0? 0 : 1;
            fdd.key_5 = (data[FDRIVE_KEYS] & 0x10) == 0? 0 : 1;
            fdd.key_6 = (data[FDRIVE_KEYS] & 0x20) == 0? 0 : 1;
            fdd.key_7 = (data[FDRIVE_KEYS] & 0x40) == 0? 0 : 1;
            fdd.key_8 = (data[FDRIVE_KEYS] & 0x80) == 0? 0 : 1;
            fdd.ladar_1 = data[FDRIVE_LADAR_1];
            fdd.ladar_2 = data[FDRIVE_LADAR_2];
            fdd.ladar_3 = data[FDRIVE_LADAR_3];
            fdd.ladar_4 = data[FDRIVE_LADAR_4];
            fdd.ladar_5 = data[FDRIVE_LADAR_5];
            fdd.ladar_6 = data[FDRIVE_LADAR_6];
            fdd.ladar_7 = data[FDRIVE_LADAR_7];
            fdd.ladar_8 = data[FDRIVE_LADAR_8];
            fdd.unknow_1 = data[FDRIVE_UNKNOW_1];
            fdd.unknow_2 = data[FDRIVE_UNKNOW_2];
            fdd.unknow_3 = data[FDRIVE_UNKNOW_3];
            fdd.unknow_4 = data[FDRIVE_UNKNOW_4];
            fdd.unknow_5 = data[FDRIVE_UNKNOW_5];
            fdd.unknow_6 = data[FDRIVE_UNKNOW_6];
            fdd.heartbeat = data[FDRIVE_HEARTBEAT];
            fdd.xor_check = data[FDRIVE_XORCHECK];
            return fdd;
        }
        void printDriveData(FdriveData fdd)
        {
            DEBUG_INFO("LEN = 0x%02x\tMODEL = 0x%02x\tSOC = 0x%02x\tERR = 0x%02x\tHEARTBEAT = 0x%02x\tXORCHECK = 0x%02x",fdd.len,fdd.model,fdd.soc,fdd.err,fdd.heartbeat,fdd.xor_check);
            DEBUG_INFO("KEYS1 = 0x%02x\tKEYS2 = 0x%02x\tKEYS3 = 0x%02x\tKEYS4 = 0x%02x",fdd.key_1,fdd.key_2,fdd.key_3,fdd.key_4);
            DEBUG_INFO("KEYS5 = 0x%02x\tKEYS6 = 0x%02x\tKEYS7 = 0x%02x\tKEYS8 = 0x%02x",fdd.key_5,fdd.key_6,fdd.key_7,fdd.key_8);
            DEBUG_INFO("LADAR_1 = 0x%02x\tLADAR_2 = 0x%02x\tLADAR_3 = 0x%02x\tLADAR_4 = 0x%02x",fdd.ladar_1,fdd.ladar_2,fdd.ladar_3,fdd.ladar_4);
            DEBUG_INFO("LADAR_5 = 0x%02x\tLADAR_6 = 0x%02x\tLADAR_7 = 0x%02x\tLADAR_8 = 0x%02x",fdd.ladar_5,fdd.ladar_6,fdd.ladar_7,fdd.ladar_8);
        	DEBUG_INFO("UNKNOW_1 = 0x%02x\tUNKNOW_2 = 0x%02x\tUNKNOW_3 = 0x%02x\tUNKNOW_4 = 0x%02x\tUNKNOW_5 = 0x%02x\tUNKNOW_6 = 0x%02x",fdd.unknow_1,fdd.unknow_2,fdd.unknow_3,fdd.unknow_4,fdd.unknow_5,fdd.unknow_6);
        }
	private:  
		std::string portName_;  
		speed_t baudRate_;  
		int fd_;  
};


// 结构体到消息体的转换(来自下位机ecu的协议数据)
void DriveFeed2MSG(FdriveData& fcmd,rads::rads_scmd& tcmd)
{
    tcmd.header.stamp = ros::Time::now();
    tcmd.header.frame_id = "nyd";
	tcmd.len = fcmd.len;
    tcmd.model = fcmd.model;
    tcmd.soc = fcmd.soc;
    tcmd.err = fcmd.err;
    tcmd.rpm_l = fcmd.rpm_l;
    tcmd.rpm_r = fcmd.rpm_r;
    tcmd.key_1 = fcmd.key_1;
    tcmd.key_2 = fcmd.key_2;
    tcmd.key_3 = fcmd.key_3;
    tcmd.key_4 = fcmd.key_4;
    tcmd.key_5 = fcmd.key_5;
    tcmd.key_6 = fcmd.key_6;
    tcmd.key_7 = fcmd.key_7;
    tcmd.key_8 = fcmd.key_8;

    tcmd.ladar_1 = fcmd.ladar_1;
    tcmd.ladar_2 = fcmd.ladar_2;
    tcmd.ladar_3 = fcmd.ladar_3;
    tcmd.ladar_4 = fcmd.ladar_4;
    tcmd.ladar_5 = fcmd.ladar_5;
    tcmd.ladar_6 = fcmd.ladar_6;
    tcmd.ladar_7 = fcmd.ladar_7;
    tcmd.ladar_8 = fcmd.ladar_8;

    tcmd.unknow_1 = fcmd.unknow_1;
    tcmd.unknow_2 = fcmd.unknow_2;
    tcmd.unknow_3 = fcmd.unknow_3;
    tcmd.unknow_4 = fcmd.unknow_4;
    tcmd.unknow_5 = fcmd.unknow_5;
    tcmd.unknow_6 = fcmd.unknow_6;
    tcmd.heartbeat = fcmd.heartbeat;
    tcmd.xor_check = fcmd.xor_check;
}


#endif
